#define WallSensor S1
#define BrickSensor S2
#define ColourSensor SENSOR_3
#define LeftMotor OUT_B
#define RightMotor OUT_C
#define BothMotors OUT_BC

/*=====Calibration==============================================================
Set the lemming specific constants here.
*/

const int straight_calibration = 3, light_threshold = 540, dark_threshold = 340;


/*====="Level -1": Integration==================================================
Sets the two motor speeds as a combination of all the above layers' controls.
*/

const int SET_MINOR_TURN = 10;
/*the Lemming turns slightly to the right when not detecting a brick, SET_MINOR_TURN
  determines how much it turns.  default = 10
*/
const int STRAIGHT = straight_calibration;
const int FULL_SPIN = 100, MINOR_TURN = STRAIGHT + (-1 * SET_MINOR_TURN);
int DriveSpeed, BaseSpeed, Wander, Spin, Sync, BaseSync = MINOR_TURN;

task Speed(){
  DriveSpeed = BaseSpeed;
  BaseSync = MINOR_TURN;
  while(true){
    Sync = (BaseSync * Wander) + Spin; //Synchro constant = base, wander(1/0), spin = (0/100)
  }
}

/*=====Level 0: Drive===========================================================
Exactly what it says on the label.
*/

task Drive(){
  while(true){
   OnFwdSync(BothMotors, DriveSpeed, Sync);
  }
}

/*=====Level 1: Dodge===========================================================
If the wall is within the wall sensor's range, spin for a little while.
*/

int wallDistance, lastReading, SpinDirection;
const int  KEEP_DIRECTION = 1, DUMP_DIRECTION = -1, HALF_SPIN = -50, COLLISION = 27;
int spinTime = 750, stuckTime = 1500, NearWall = COLLISION;
bool collision;

task Dodge(){
  collision = false;  SpinDirection = DUMP_DIRECTION;
  while(true){
    until (collision ||  (SensorUS(WallSensor) < NearWall));
    Wait(50);
    if (collision ||  (SensorUS(WallSensor) < NearWall)){ //double check to make sure it is not a US error!
      collision = true; Wander = 0;
      DriveSpeed *= SpinDirection; Spin = SpinDirection * HALF_SPIN;
      Wait(spinTime);
      Wander = 1; DriveSpeed = BaseSpeed;
      Spin = STRAIGHT; SpinDirection = DUMP_DIRECTION;
      NearWall = COLLISION;
      collision = false;
    }
   }
}


task CheckStuck()
{
  while(true){
  lastReading = SensorUS(WallSensor);
  Wait(stuckTime);
  if(lastReading == SensorUS(WallSensor)) collision = true;
  }
 }


/*=====Level 2: Seek============================================================
If the brick sensor detects a brick, straighten out. The sensor is forward-
mounted, so this will result in a brick attraction.
*/


int brickthreshold = 120, brickDistance, straightTime = 1000;

task Seek(){
  while(true){
    if(SensorUS(BrickSensor) < brickthreshold){
       Wander = 0; Wait(straightTime);
    }
    else Wander = 1;
  }
}




/*=====Level 3: seeBricks========================================================
If carrying a dark brick, drop it at other bricks but keep it at walls.
If carrying a light brick, drop it at walls but keep it at other bricks.
*/

//to cope with innacuracy of sensor at close range.. collision detected from further away.. drive straight, then, dump

const int FAR_COLLISION = 65;
int noBrick = dark_threshold, lightBrick = light_threshold, brickCollision = 30;
int currentBrick, wallFar = FAR_COLLISION, untilDump = 500;//500 works well! will 100 work better??
bool foundDark, foundLight;

task seeBrick(){
  while(true){
    until(ColourSensor > noBrick);
    if (ColourSensor > lightBrick){
       SpinDirection = DUMP_DIRECTION; foundLight = true;
       NearWall = COLLISION; //trying this out
    }
    else if((!collision) && (SensorUS(BrickSensor) <= brickCollision) &&  (SensorUS(WallSensor) > wallFar)){
       PlayTone(587,200);
       Wander = 0; Wait(untilDump);
       if (ColourSensor < lightBrick){   //double check to make sure it is not a light brick
         SpinDirection = DUMP_DIRECTION; collision = true; foundDark = true;
         }
       until((!collision) || (ColourSensor > lightBrick));
    }
    else {
       SpinDirection = KEEP_DIRECTION;
       NearWall = FAR_COLLISION; //trying this out! i think this really helps!!
      }
  }
}

/*=====KeepAwake================================================================
this task resets the sleep timer so the lemming does not shut off automatically!
careful though, will run till it dies!
*/

const int TEN_MINUTES =36000000;

task keepAwake()
{
  while(true)
  {
    Wait(TEN_MINUTES);
    ResetSleepTimer();
  }
}




/*=====Main Task==============================================================*/

task main(){
  SetSensorType(WallSensor, SENSOR_TYPE_LOWSPEED);
  SetSensorType(BrickSensor, SENSOR_TYPE_LOWSPEED);
  SetSensorType(S3, SENSOR_TYPE_LIGHT_ACTIVE);
  SetSensorMode(S3, SENSOR_MODE_RAW);
  Wander = 1;
  Spin = 0;
  BaseSpeed = 65;

  start Speed;
  start Drive;
  start Dodge;
  start CheckStuck;
  start Seek;
  start seeBrick;
  start keepAwake;
}